//
// Created by 32513 on 25-4-7.
//

#include "motor.h"
#include "gpio.h"
#include "tim.h"




void Motor_Init(void)
{


  HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_1);
  HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_2);
  HAL_TIM_Encoder_Start(&htim5, TIM_CHANNEL_1);
  HAL_TIM_Encoder_Start(&htim5, TIM_CHANNEL_2);

  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);

}


void Get_Motor_Speed(motor_measure_t* motor_measure)
{
  static float speed_temp = 0;
  motor_measure->now_time = HAL_GetTick();
  motor_measure->Direction = __HAL_TIM_IS_TIM_COUNTING_DOWN(motor_measure->motor_htim);
  motor_measure->enc1 = (uint32_t)(__HAL_TIM_GET_COUNTER(motor_measure->motor_htim));	//获取定时器的值
  if((motor_measure->Direction == 0) &(motor_measure->enc1 < motor_measure->enc1_old))				//正向旋转数值变小,说明进位
  {
    motor_measure->enc2++;
  }
  if((motor_measure->Direction == 1) &(motor_measure->enc1 > motor_measure->enc1_old))				//反向旋转数值变小,说明借位
  {
    motor_measure->enc2--;
  }
  motor_measure->enc = motor_measure->enc2<<16 | motor_measure->enc1;								//计算当前计数总值，带+-号
  motor_measure->error_enc = (motor_measure->enc - motor_measure->last_enc);
  motor_measure->error_time = (motor_measure->now_time - motor_measure->last_time);
  speed_temp = (float)(motor_measure->enc - motor_measure->last_enc)/(float)(motor_measure->now_time - motor_measure->last_time)/528.0f*1000.0f;
  if(motor_measure->motor_htim == &htim5)
    speed_temp = -speed_temp; //tim5也就是右边的电机在使用自制开发板后测量的方向和实际方向相反，所以反转测量值
  if(speed_temp < 50 && speed_temp > -50)
    motor_measure->Speed = speed_temp;
  motor_measure->enc1_old = motor_measure->enc1;									//更新enc1_old，便于下次计算
  motor_measure->last_time = motor_measure->now_time;        	//更新last_time，便于下次计算
  motor_measure->last_enc = motor_measure->enc;
}
/**
 *
  * @brief  控制电机转及方向
  * @param Speed +-100，浮点型
  * @param motor_htim 电机PWM定时器句
  * @param channel 控制电机的PWM通道
  * @retval None
  */
void Motor_Control(TIM_HandleTypeDef *motor_htim, uint32_t channel,float Speed)
{
  if(Speed > 0)
  {
    if(channel == TIM_CHANNEL_2)
    {
      HAL_GPIO_WritePin(GPIOC, GPIO_PIN_2, GPIO_PIN_RESET);
      HAL_GPIO_WritePin(GPIOC, GPIO_PIN_3, GPIO_PIN_SET);
      __HAL_TIM_SetCompare(motor_htim, TIM_CHANNEL_2, (int)(Speed*10.0f));
    }
    if(channel == TIM_CHANNEL_3)
    {
      HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, GPIO_PIN_RESET);
      HAL_GPIO_WritePin(GPIOC, GPIO_PIN_5, GPIO_PIN_SET);
      __HAL_TIM_SetCompare(motor_htim, TIM_CHANNEL_3, (int)(Speed*10.0f));

    }
  }
  else if(Speed < 0)
  {
    Speed = Speed * -1;
    if(channel == TIM_CHANNEL_2)
    {
      HAL_GPIO_WritePin(GPIOC, GPIO_PIN_2, GPIO_PIN_SET);
      HAL_GPIO_WritePin(GPIOC, GPIO_PIN_3, GPIO_PIN_RESET);
      __HAL_TIM_SetCompare(motor_htim, TIM_CHANNEL_2, (int)(Speed*10.0f));
    }
    if(channel == TIM_CHANNEL_3)
    {
      HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, GPIO_PIN_SET);
      HAL_GPIO_WritePin(GPIOC, GPIO_PIN_5, GPIO_PIN_RESET);
      __HAL_TIM_SetCompare(motor_htim, TIM_CHANNEL_3, (int)(Speed*10.0f));
    }
  }
  else
  {
    if(channel == TIM_CHANNEL_2)
    {
      HAL_GPIO_WritePin(GPIOC, GPIO_PIN_2, GPIO_PIN_RESET);
      HAL_GPIO_WritePin(GPIOC, GPIO_PIN_3, GPIO_PIN_RESET);
      __HAL_TIM_SetCompare(motor_htim, TIM_CHANNEL_2, (int)(Speed*10.0f));
    }
    if(channel == TIM_CHANNEL_3)
    {
      HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, GPIO_PIN_RESET);
      HAL_GPIO_WritePin(GPIOC, GPIO_PIN_5, GPIO_PIN_RESET);
      __HAL_TIM_SetCompare(motor_htim, TIM_CHANNEL_3, (int)(Speed*10.0f));
    }
  }
  // HAL_GPIO_WritePin(GPIOC, GPIO_PIN_2, GPIO_PIN_SET);
  // HAL_GPIO_WritePin(GPIOC, GPIO_PIN_3, GPIO_PIN_RESET);
  // __HAL_TIM_SetCompare(motor_htim, channel, /*(int)(Speed/100.0f*10.0f)*/999);

}